/*
* 时间：2021.07.22
* 代码来源：公众号【从0到1的点云】
* 功能：根据高度渲染点云
*/

#include<iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/common.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/filter.h>
#include<opencv2\opencv.hpp>
#include  <pcl/keypoints/uniform_sampling.h>

std::vector<float> normal_x(const std::vector<float> x, const float& y_min, const float &y_max)
{
  std::vector<float> result;
  double  x_max = x[0], x_min = x[0];
  for (int i = 0; i < x.size(); i++)
  {
    if (x[i] > x_max)
    {
      x_max = x[i];
    }
    if (x[i] < x_min)
    {
      x_min = x[i];
    }
  }
  for (auto & temp_x : x)
  {
    //y = (ymax - ymin)*(x - xmin) / (xmax - xmin) + ymin;
    float u = (y_max - y_min)*(temp_x - x_min) / (x_max - x_min) + y_min;
    result.push_back(u);
  }

  return result;
}
int main()
{
  pcl::PCDReader reader;
  pcl::PointCloud<pcl::PointXYZ> input_cloud1;
  pcl::PointCloud<pcl::PointXYZ> input_cloud;
  pcl::PointXYZ min_point;
  pcl::PointXYZ max_point;
  reader.read("tree02.pcd", input_cloud1);
  std::vector<int> idx;
  pcl::removeNaNFromPointCloud(input_cloud1, input_cloud1, idx);

  //降采样
  pcl::PointCloud<int> sampled_indices;
  pcl::UniformSampling<pcl::PointXYZ> uniform_sampling;
  uniform_sampling.setInputCloud(input_cloud1.makeShared());
  uniform_sampling.setRadiusSearch(0.1);
  uniform_sampling.filter(input_cloud);

  pcl::getMinMax3D((input_cloud), min_point, max_point);

  //按z赋予颜色
  double   z_min = min_point.z;
  double  z_max = max_point.z;
  double  z_mid = fabs(z_min + z_max) / 2.0;
  double  z_maxmin_range = fabs(z_max - z_min);
  double  z_midmin_range = fabs(z_mid - z_min);
  double  z_maxmid_range = fabs(z_max - z_mid);
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
  std::vector<float> z;
  for (const auto &point : input_cloud)
  {
      z.push_back(point.z);
  }
  std::vector<float> normal_z = normal_x(z, 0, 255);//归一化z值
  //利用opencv赋予颜色
  for (auto point : input_cloud)
  {
    static int count = 0;
    cv::Mat temp(cv::Size(1, 1),CV_8UC1);
    temp.at<cv::Vec3b>(0, 0) = normal_z[count];
    cv::Mat im_color;
    cv::applyColorMap(temp, im_color,cv::COLORMAP_JET);
    pcl::PointXYZRGB temp_point;

    temp_point.x = point.x;
    temp_point.y = point.y;
    temp_point.z = point.z;
    temp_point.b = im_color.at<cv::Vec3b>(0, 0)[0];
    temp_point.g = im_color.at<cv::Vec3b>(0, 0)[1];
    temp_point.r = im_color.at<cv::Vec3b>(0, 0)[2];
    output_cloud->push_back(temp_point);
    std::cout << count << std::endl;
    count++;
  }
  //可视化
  pcl::visualization::PCLVisualizer viewer("按高程赋予颜色");
  pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(output_cloud);
  viewer.addPointCloud<pcl::PointXYZRGB>(output_cloud, rgb, "cloud");//cloud is pointer
  viewer.spin();
}